#include <DeformingPolygon.h>
#include <map>
#include <szMiscOperations.h>
#include <szMexUtility.h>
#include <IntersectionConvexPolygons.h>

/*
A unit vector from o to x.
*/
CParticleF 
NormalizedDirection(CParticleF& x, CParticleF& o)
{
	float dx = x.m_X - o.m_X;
	float dy = x.m_Y - o.m_Y;
	float len = sqrt(dx*dx + dy*dy);
	if(len > 1.0e-5)
	{
		return CParticleF(dx/len, dy/len);
	}
	else
	{
		return CParticleF(0.0f, 0.0f);
	}
}

/*
return a unit vector that bisects the angle formed by three points: a-o-b.
*/
CParticleF
bisector(CParticleF& o, CParticleF& a, CParticleF& b)
{
	CParticleF x = NormalizedDirection(a, o);
	CParticleF y = NormalizedDirection(b, o);
	CParticleF z((x.m_X+y.m_X)/2, (x.m_Y+y.m_Y)/2);
	float vx = z.m_X;
	float vy = z.m_Y;
	float len0 = sqrt(vx*vx + vy*vy);
	vx = vx / len0;
	vy = vy / len0;
	return CParticleF(vx, vy);
}

float
intersectPlaneAndLine(CParticleF op, CParticleF& z, CParticleF& ol, CParticleF u)
{
	//move the origin to ol.
	op.m_X -= ol.m_X;
	op.m_Y -= ol.m_Y;
	op.m_Z -= ol.m_Z;
	float t = (z.m_X * op.m_X + z.m_Y * op.m_Y + z.m_Z * op.m_Z) / (z.m_X * u.m_X + z.m_Y * u.m_Y + z.m_Z * u.m_Z);
	return t;
}

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, float precision = 1.0e-3)
{
	float d = Distance2Line(a, c, b);
	return d < precision;
}

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, CParticleF& d, float precision = 1.0e-3)
{
	float d1 = Distance2Line(a, c, b);
	float d2 = Distance2Line(a, c, d);
	return d1 < precision && d2 < precision;
}

CParticleF
crossProduct(CParticleF& a, CParticleF& b)
{
	return CParticleF(a.m_Y*b.m_Z-a.m_Z*b.m_Y, a.m_Z*b.m_X-a.m_X*b.m_Z, a.m_X*b.m_Y-a.m_Y*b.m_X);
}

bool 
MovingParticle::setVelocity()
{
	if(next && prev)
	{
		CParticleF a = prev->p;
		CParticleF b = next->p;
		CParticleF bs = bisector(p, a, b);
		float ang = GetVisualAngle2(a.m_X, a.m_Y, b.m_X, b.m_Y, p.m_X, p.m_Y);
		float cs = cos((PI - Abs(ang)) / 2.0);
		float len = 1.0f / cs;
		bs.m_X *= len;
		bs.m_Y *= len;
		if(ang<0)
		{
			bs.m_X = -bs.m_X;
			bs.m_Y = -bs.m_Y;
		}
		v[0] = bs.m_X;
		v[1] = bs.m_Y;
		return true;
	}
	else
	{
		return false;
	}
}


DeformingPolygon::DeformingPolygon(vector<CParticleF>& pnts)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	float eps = 1.0e-5;
	for(int i=0; i<pnts.size(); ++i)
	{
		int i0 = (i-1+pnts.size()) % pnts.size();
		int i2 = (i+1) % pnts.size();
		CParticleF a = pnts[i0];
		CParticleF b = pnts[i2];
		CParticleF o = pnts[i];

		CParticleF bs = bisector(o, a, b);
		float ang = GetVisualAngle2(a.m_X, a.m_Y, b.m_X, b.m_Y, o.m_X, o.m_Y);
		if(Abs(ang) < eps)
		{
			//sharp turn at a lean node
			//duplicate points and set the velocity to away directions 
			float theta = GetVisualDirection(o.m_X, o.m_Y, a.m_X, a.m_Y);
			float t1 = theta - PI/4.0;
			float t2 = theta + PI/4.0;
			MovingParticle* p1 = factory->makeParticle(o);
			MovingParticle* p2 = factory->makeParticle(o);
			p1->v[0] = cos(t1);
			p1->v[1] = sin(t1);
			p2->v[0] = cos(t2);
			p2->v[1] = sin(t2);
			particles.push_back(p1);
			particles.push_back(p2);
		}
		else
		{
			float cs = cos((PI - Abs(ang)) / 2.0);
			float len = 1.0f / cs;
			bs.m_X *= len;
			bs.m_Y *= len;
			if(ang<0)
			{
				bs.m_X = -bs.m_X;
				bs.m_Y = -bs.m_Y;
			}
			MovingParticle* p = factory->makeParticle(o);
			p->v[0] = bs.m_X;
			p->v[1] = bs.m_Y;
			particles.push_back(p);
		}
	}
	for(int i=0; i<particles.size(); ++i)
	{
		particles[i]->p0 = particles[i]->p;
		int j = (i+1) % particles.size();
		particles[i]->next = particles[j];
		int k = (i-1+particles.size()) % particles.size();
		particles[i]->prev = particles[k];
	}
}

float
DeformingPolygon::nextEdgeEvent()
{
	float t = std::numeric_limits<float>::infinity();
	for(int i=0; i<particles.size(); ++i)
	{
		float xi = particles[i]->p.m_X + particles[i]->v[0];
		float yi = particles[i]->p.m_Y + particles[i]->v[1];
		MovingParticle* q = particles[i]->next;
		float xj = q->p.m_X + particles[j]->v[0];
		float yj = q->p.m_Y + particles[j]->v[1];
		pair<float,float> param = _IntersectConvexPolygon::intersect(particles[i]->p, CParticleF(xi,yi), q->p, CParticleF(xj,yj));
		float t0 = Max(param.first, param.second);
		if(t0>=0 && t0 < t)
		{
			t = t0;
		}
	}
	return t;
}

float
DeformingPolygon::nextSplitEvent()
{
	float t = std::numeric_limits<float>::infinity();
	for(int i=0; i<particles.size(); ++i)
	{
		CParticleF bs = bisector(particles[i]->p, particles[i]->prev->p, particles[i]->next->p);
		if(bs.m_X * particles[i]->v[0] + bs.m_Y * particles[i]->v[1] < 0) //concave
		{
			CParticleF ol = particles[i]->p;
			CParticleF ul(particles[i]->v[0], particles[i]->v[1], 1.0);
			for(int j=0; j<particles.size(); ++j)
			{
				if(particles[i]==particles[j] || particles[i]->next==particles[j] || particles[i]->prev==particles[j]) continue;
				CParticleF op = particles[j]->p;
				CParticleF oq = particles[j]->next->p;
				CParticleF ov = NormalizedDirection(op, oq);
				CParticleF up0(ov.m_Y, -ov.m_X, 1.0f);
				CParticleF up = crossProduct(ov, up0);
				if(up.m_X*particles[j]->v[0] + up.m_Y*particles[j]->v[1] < 0)
				{
					up.m_X = -up.m_X;
					up.m_Y = -up.m_Y;
				}
				float t0 = intersectPlaneAndLine(op, up, ol, ul);
				CParticleF loc(ol.m_X+t*ul.m_X, ol.m_Y+t*ul.m_Y, ol.m_Z+t*ul.m_Z);
				if(t0 >= 0 && t0<t) 
				{
					//check if the intersection point is within the sweep of the line segment.
					CParticleF op2 = particles[j]->move(t0); 
					CParticleF oq2 = particles[j]->next->move(t0); 
					if(Distance2LineSegment(op2,  oq2, loc)<1.0f) //1.0f is arbitrary. I thought it is small enough but maybe too big.
					{
						t = t0;
					}
				}
			}
		}
	}
	return t;
}

bool 
DeformingPolygon::fixPolygon(vector<DeformingPolygon>& additions, float eps)
{
	ParticleFactory* factory = ParticleFactory::getInstance();
	//find any duplicates (as the result of edge events)
	while(true)
	{
		bool bChanged = false;
		for(int i=0; i<particles.size(); ++i)
		{
			if(Distance(particles[i]->p, particles[i]->next->p) < eps)
			{
				MovingParticle* p = factory->makeParticle(particles[i]->p);
				p->prev = particles[i]->prev;
				p->next = particles[i]->next->next;
				particles.erase(particles.begin()+i);
				particles.insert(particles.begin()+i, p);
				vector<MovingParticle*>::iterator pn = find(particles.begin(), particles.end(), particles[i]->next);
				particles.erase(pn);
				bChanged = true;
				break;
			}
		}
	}
}


DeformingPolygon
traceTree(vector<Vertex<CParticleF>*>& tree)
{
	map<Vertex<CParticleF>*,int> imap;
	for(int i=0; i<tree.size(); ++i)
	{
		imap[tree[i]] = i;
	}
	vector<CParticleF> points;
	vector<int> numVisits(tree.size(), 0);
	Vertex<CParticleF>* start = tree[0];
	Vertex<CParticleF>* p = start;
	Vertex<CParticleF>* prev =  NULL;;
	while(true)
	{
		points.push_back(p->key);
		numVisits[imap[p]] ++;
		Vertex<CParticleF>* next =  NULL;
		float minang = 2*PI;
		for(int i=0; i<p->aList.size(); ++i)
		{
			Vertex<CParticleF>* v = p->aList[i]->v;
			if(numVisits[imap[v]] < v->aList.size())
			{
				if(prev==NULL) next = v;
				else
				{
					float ang = GetVisualAngle2(prev->key.m_X, prev->key.m_Y, v->key.m_X, v->key.m_Y, p->key.m_X, p->key.m_Y);
					if(ang < 0) ang += 2*PI;
					if(ang < minang)
					{
						minang = ang;
						next = v;
					}
				}
			}
		}
		if(next == NULL) break;
		prev = p;
		p = next;
	}
	return DeformingPolygon(points);
}
